package edu.wpi.first.wpilibj.frc;

import edu.wpi.first.wpilibj.util.Point3D;
import edu.wpi.first.wpilibj.util.Rectangle3D;
import java.util.ArrayList;
import java.util.Iterator;

/**
 * Represents a virtual field that the robot is to play on.
 * Supports adding {@link Rectangle3D} as object bounds.
 * @author Dylan Frese
 */
public class VirtualField {

    private double width, depth;
    
    private ArrayList<Rectangle3D> objects, bridges;

    /**
     * Create a new field with the specified width, depth, and list of objects
     * that occupy its space. It also adds the contents of 'bridges' to a separate
     * set.
     * @param width The width of the field
     * @param depth The height of the field
     * @param objects Any physical objects that occupy the field.
     * @param bridges The rectangles representing bridges. The y is the bottom 
     * of the bridge, and height expands upwards to the max height of the bridge.
     */
    public VirtualField(double width, double depth, Rectangle3D[] objects, Rectangle3D[] bridges) {
        this.objects = new ArrayList(java.util.Arrays.asList(objects));
        this.bridges = new ArrayList(java.util.Arrays.asList(bridges));
        this.width = width;
        this.depth = depth;
    }

    /**
     * Tests weather the {@link Rectangle3D} collides (or intersects) any of the
     * objects in the field.
     * @param tester The rectangle to test for intersection
     * @return whether 'tester' intersects with anything.
     */
    public boolean col(Rectangle3D tester) {
        for (Rectangle3D r : objects) {
            if (r.intersects(tester)) {
                return true;
            }
        }
        return false;
    }
    
    public boolean isOnBridge(Point3D p){
        for (Rectangle3D r : bridges) {
            if (r.contains(p)) {
                return true;
            }
        }
        return false;        
    }

    /**
     * Get an array of all the {@link Rectangle3D}s occupying space in the VirtualField
     * @return an array of three dimensional Rectangles that occupy space in this VirtualField
     */
    public Rectangle3D[] getRectangles() {
        Rectangle3D[] returnarray = new Rectangle3D[objects.size()];
        int x = 0;
        for (Iterator<Rectangle3D> it = objects.iterator();it.hasNext();) {
            returnarray[x] = it.next();
            x++;
        }
        return returnarray;
    }

    /**
     * Get the depth of the field as a double
     * @return The depth of the field
     */
    public double getDepth() {
        return depth;
    }

    /**
     * Get the width of the field as a double
     * @return The width of the field
     */
    public double getWidth() {
        return width;
    }
    
    /**
     * Add a rectangle to the set
     * @param r The rectangle to add
     */
    public void addRectangle(Rectangle3D r){
        objects.add(r);
    }
    
    /**
     * Remove a rectangle 
     * @param r The {@link Rectangle3D} to remove from the set of rectangles in the VirtualField
     * @return whether or not the rectangle was in the set before this method's call.
     */
    public boolean removeRectangle(Rectangle3D r){
        return objects.remove(r);
    }
    
}
